program program_psr
use madx_ptc_module
use pointer_lattice
implicit none
type(layout), pointer:: psr
type(damap) id,m,a_l,dri
type(real_8) ray(6)
real(dp) closed_orbit(6),sip
type(internal_state) state
type(normalform) normal
integer :: no1=0,np1=0 ,ind(4)=0,i,k,pur
type(taylor) average_floquet,average_xp
type(taylor) p(2),phase_advance(2),a11,a12
logical(lp) mis
type(fibre), pointer :: f

    interface
       subroutine build_PSR(PSR,MIS)
         use madx_ptc_module
         use pointer_lattice
         implicit none
         type(layout), target :: PSR
         logical(lp) mis
       end subroutine build_PSR
    end interface

call ptc_ini

psr=>m_u%start

Write(6,*) " small misalignments and rotations in lattice ? input t or f "
read(5,*) mis

call build_PSR(PSR,mis)

!!!  Phase space of Taylor maps will be (x,px,y,py) with delta dependence
!!! time is ignored in FPP but computed in PTC as ray(6)

state=delta0 

!do while(no1<=0) 
! write(6,*) " input an order > 0 (not too big first)! "
! read(5,*) no1
!enddo
no1=2

np1=0    !!!  No system parameters in the map

call init(state,no1,np1) ! PTC and FPP are properly initialized

write(6,*) " number of degrees of freedom ",c_%nd
write(6,*) 

call alloc(id,m,a_l,dri)  
call alloc(ray)
call alloc(normal)
call alloc(average_floquet,average_xp,a11,a12)
call alloc(p)
call alloc(phase_advance)
 

closed_orbit=0.d0 ! initial guess for closed orbit

call find_orbit(psr,closed_orbit,1,state,1.d-5)

write(6,'(a16,4(1x,g12.5))') " closed orbit = ",closed_orbit(1:4)

id=1;   ray=closed_orbit+id;   ! ray= closed orbit + identity map

call track(psr,ray,1,state) ! One turn map is computed via the ray

!!! The ray is really the result of a truncated power series algebra
!!! The map we are interested to compute is around the closed orbit: 
!!  it is really part of a diffential algebra
m=ray   !   The ray is "officially" turned into a "damap"  (DA in Berz's honour)

normal = m  ! The map is normalised   

a_l=normal%a_t

Write(6,*) " Do you want to purify the output of the normal form "
Write(6,*) " if you say no (input 0), then the chromaticity will be wrong "
Write(6,*) "  because my normal form does not deal, beyond the linear part,  "
Write(6,*) "  with the Courant-Snyder condition "
Write(6,*)
Write(6,*) " Enter 1 for purification, 0 otherwise "
read(5,*) pur
!!!!  Algorithm to force the linear A into the !!!!
!!!!  form A_12=A_34=0                         !!!!
if(pur==1)  call purify(a_l,p)
!!!!          End of the Algorithm             !!!!
 
ray=closed_orbit+a_l  ! Put the new A into RAY(6)

f=>psr%start

phase_advance(1)=0.d0;phase_advance(2)=0.d0;

do k=1,psr%n

 ray=a_l+closed_orbit

 call track(psr,ray,k,k+1,state)

 a_l=ray

!!!!  Algorithm to force the linear A into the !!!!
!!!!  form A_12=A_34=0                         !!!!
 call purify(a_l,p)
!!!!          End of the Algorithm             !!!!   
 
    closed_orbit=ray

    phase_advance(1)=phase_advance(1)+p(1)/twopi
    phase_advance(2)=phase_advance(2)+p(2)/twopi

f=>f%next
enddo

write(6,*) 
write(6,*) " Fractional Tunes in both planes from Normal Form"
write(6,*) normal%tune(1:2)
write(6,*) " Chromaticities in both planes from Normal Form "
write(6,*) (normal%dhdj%v(1).sub.'00001'),(normal%dhdj%v(2).sub.'00001')
write(6,*) 
write(6,*) " Phase advances in both planes "
write(6,*) (phase_advance(1).sub.'0'),(phase_advance(2).sub.'0')
write(6,*) " Chromaticities in both planes from Phase Advance "
if(pur==0) then
write(6,*)
write(6,*) ' Slightly incorrect because you did not "purify"   '
write(6,*) ' the initial A_L '
endif
write(6,*) (phase_advance(1).sub.'00001'),(phase_advance(2).sub.'00001')



 




call kill(id,m,a_l,dri)  
call kill(ray)
call kill(normal)
call kill(average_floquet,average_xp,a11,a12)
call kill(p)
call kill(phase_advance)


write(6,*) "   "
write(6,*) " hit return to terminate program "
write(6,*) "   "
pause 
 
end program program_psr



!=================================================================
subroutine  purify(a_l,p)
use madx_ptc_module
use pointer_lattice
implicit none
type(taylor), intent(inout) :: p(2)
type(damap), intent(inout) :: a_l
type(damap) dri
type(taylor) a11,a12
integer ind(4),i,f

call alloc(dri)
call alloc(a11,a12)

   do i=1,2
   ind=0; ind(2*i-1)=1;
   a11=(a_l%v(2*i-1).par.ind)
   ind=0; ind(2*i)=1;
   a12=(a_l%v(2*i-1).par.ind)
 
          p(i)=atan2(a12,a11) 


    dri%v(2*i-1) = cos(p(i))*(1.0_dp.mono.(2*i-1))-sin(p(i))*(1.0_dp.mono.(2*i))
    dri%v(2*i)   = cos(p(i))*(1.0_dp.mono.(2*i))  +sin(p(i))*(1.0_dp.mono.(2*i-1))
  enddo

    a_l=a_l*dri


call kill(dri)
call kill(a11,a12)

end subroutine  purify

subroutine  build_PSR(PSR,mis)
use madx_ptc_module
use pointer_lattice
implicit none

type(layout), target :: PSR

real(dp) :: ang, brho, kd, kf, Larc,SG(6),cut
type(fibre) :: b, d1, d2, qd, qf
type(layout) :: cell
logical(lp) mis
!-----------------------------------

call make_states(.false.)
exact_model = .true.
default = default + nocavity  
call update_states
madlength = .false.

ang = (twopi * 36.d0 / 360.d0)
Larc = 2.54948d0
brho = 1.2d0 * (Larc / ang)
call set_mad(brho = brho, method = 2, step = 10)
madkind2 = drift_kick_drift

kf =  2.72d0 / brho
kd = -1.92d0 / brho

d1 = drift("D1", 2.28646d0)
d2 = drift("D2", 0.45d0)
qf = quadrupole("QF", 0.5d0, kf)
qd = quadrupole("QD", 0.5d0, kd)
b  = rbend("B", Larc, ang)
cell = d1 + qd + d2 + b + d2 + qf + d1

PSR = 10 * cell
PSR = .ring.PSR

call survey(PSR)

if(mis) then
 sig=1.d-5
 cut=4.d0
 call MESS_UP_ALIGNMENT(PSR,SIG,cut)
endif
end subroutine build_PSR
